# -*- coding: utf-8 -*-
import threading
import time
import socket
import rospy
import moveit_msgs.msg
from transform import *
from ti5_kinematic import Ti5Kinematic
#from Speed.msg import Speed   # 将订阅str_message话题，自定义消息类型Speed::Speed
from std_msgs.msg import String
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
Vc=-1 #需要在ROS和socket中传递或使用的变量设为全局变量
class Server(object):
    def __init__(self):
        rospy.init_node('position_pubscriber', anonymous=True)   # ROS节点初始化
        self.pub = rospy.Publisher('ik_result', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
        self.arm_kinematic = Ti5Kinematic()
        self.last_position=[0,0,0,0,0,0]
        self.start_point=[0,0,0,0,0,0]

        group_name = "armgroup"
        self.group = moveit_commander.MoveGroupCommander(group_name)

    def InfoCallback(self,msg):#ROS订阅的回调函数
        global Vc
        Vc=msg.carspeed   #订阅本车的速度

    def send_info(self,client): #SOCKET的发送函数
        global Vc
        while(1):
            if Vc>=0:
                hvjson=Vc #将vc发送出去
                client.sendall(hvjson.encode("utf-8"))
                time.sleep(1)

    def list_get_min(self,li):
        each_cha = 0
        for i in range(4):
            sta = 0
            if li[i]>0 and self.last_position[i]>0:
                sta=1
            if li[i]<0 and self.last_position[i]<0:
                sta=1
            if sta==1:
                each_cha += abs(self.last_position[i]-li[i])
            elif sta==0:
                each_cha += abs(self.last_position[i])+abs(li[i])
            #print(str(each_cha))
        return each_cha

    def recv_info(self,client): #SOCKET的接收函数
        global data
        while(1):
            #try:
            if 1>0:
                end_point=[0,0,0,0,0,0]

                info=client.recv(2000)
                data=info.decode("utf-8")   #接收到的数据
                #self.pub.publish(data)  #send to rostopic  <<charter>>
                if data[0]=="r":
                    data = data[1:]
                    li_1 = data.split(",")
                    li_2 = li_1[0].split(" ")
                    li = map(float,li_2)

                    end_point=li
                    #print("end_point"+str(end_point))
                elif data[0]=="j":
                    data = data[1:]
                    li_1=data.split(",")
                    li_2 = li_1[0].split(" ")
                    li = map(float,li_2)
                    target_pose = self.arm_kinematic.forward_kinematic_v2(li)
                    #candi_joint_angle_list = self.arm_kinematic.inverse_kinematic(target_pose)
                    
                    end_point[0]=target_pose.x
                    end_point[1]=target_pose.y
                    end_point[2]=target_pose.z
                    end_point[3]=target_pose.roll
                    end_point[4]=target_pose.pitch
                    end_point[5]=target_pose.yaw
                    #print("end_point"+str(end_point))


                if end_point:
                    """
                    pose_target= geometry_msgs.msg.Pose()
                    pose_target.position.x=end_point[0]
                    pose_target.position.y=end_point[1]
                    pose_target.position.z=end_point[2]
                    pose_target.orientation.x=end_point[3]
                    pose_target.orientation.y=end_point[4]
                    pose_target.orientation.z=end_point[5]
                    pose_target.orientation.w=0.479

                    """
                    #get start point
                    """
                    curr_p = self.group.get_current_pose().pose.position
                    curr_rpy = self.group.get_current_rpy()
                    start_point=[curr_p.x,curr_p.y,curr_p.z,curr_rpy[0],curr_rpy[1],curr_rpy[2]]
                    """
                    
                    #print("starting point:",str(start_point))


                    end_traj_point_list =list()
                    for i in range(6): 
                        if self.start_point[i]>0 and end_point[i]>0:
                            per_distance  = abs(self.start_point[i]-end_point[i])/20
                        elif self.start_point[i]<0 and end_point[i]<0:
                            per_distance  = abs(self.start_point[i]-end_point[i])/20
                        else:
                            per_distance  = abs(self.start_point[i])+abs(end_point[i])
                            per_distance = per_distance/20

                        li_tmp = list()
                        if self.start_point[i]>end_point[i]:
                            for ii in range (20):
                                tt = ii+1
                                li_tmp.append(self.start_point[i]-per_distance*tt)
                        else:    
                            for ii in range (20):
                                tt = ii+1
                                li_tmp.append(self.start_point[i]+per_distance*tt)

                        
                        end_traj_point_list.append(li_tmp)

                    self.start_point = end_point

                    for iii in range(20):
                        #print(str(end_traj_point_list[0][iii])+str(end_traj_point_list[1][iii])+str(end_traj_point_list[2][iii])+str(end_traj_point_list[3][iii])+str(end_traj_point_list[4][iii])+str(end_traj_point_list[5][iii]))
                        pose = Pose()    
                        pose.set_position(end_traj_point_list[0][iii],end_traj_point_list[1][iii],end_traj_point_list[2][iii])
                        pose.set_euler_angle(end_traj_point_list[3][iii],end_traj_point_list[4][iii],end_traj_point_list[5][iii])

                        candi_joint_angle_list_final = self.arm_kinematic.inverse_kinematic(pose)
                        if candi_joint_angle_list_final:
                            cha=10
                            res_select=0 #the result min total_cha list's number
                            numnum = 0
                            for t in candi_joint_angle_list_final:
                                now_cha = self.list_get_min(t)
                                if now_cha<cha:
                                    cha=now_cha
                                    res_select = numnum
                                numnum+=1
                            print(str(candi_joint_angle_list_final[res_select]))    


                        

            #except:
            #    client.close()


                    #plan  = self.group.plan(pose_target)
                    #plan_points_list = plan.joint_trajectory.points



if __name__ == '__main__':
#先开启了ROS
    ser = Server()
    #rospy.Subscriber("str_message", Speed, InfoCallback)   # 创建一个Subscriber，订阅名为/str_message的topic，注册回调函数InfoCallback
 
 #再创建了SOCKET       
    g_socket_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  #创建socket
    g_socket_server.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,1)
    g_socket_server.bind(("", 1605))     #关联相应的地址和端口号
    g_socket_server.listen(128)          #开始监听
    client, _ = g_socket_server.accept()    #接收并建立与客户端的连接，程序在此处开始阻塞，只有等到客户端连接进来开始往下执行
    threads = []
    t1 = threading.Thread(target=ser.send_info, args=(client,))  #开启线程
    threads.append(t1)
    t2 = threading.Thread(target=ser.recv_info, args=(client,))
    threads.append(t2)
    for t in threads:
        print "1212121"
        t.start()
        time.sleep(1)


